// main.c

// standard libs
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

#include "main.h"
#include "../utils/LCD_driver.h"
#include "../utils/button.h"
#include "../utils/util.h"

// Motor control circuit is connected to PortB  
//                              +---+---+---+---+---+
//            Port B            | 1 | 3 | x | x | V |  
//     Motor Contorl pins       +---+---+---+---+---+
//                              | 0 | 2 | x | x | G |  
//                              +---+---+---+---+---+
//  PB0 - Left motor PWM
//  PB1 - Right motor PWM
//  PB2 - Left motor Dir
//  PB3 - Right motor Dir
//
//
// Sensors are connected to ADC4(PF4) thru ADC7(PF7).  Port F is also known as the JTAG port and 
// JTAG must be disabled to use these pins.  The pins are not numbered the same as the other connectors.
//                              +---+---+---+---+---+
//            Port F  (JTAG)    | x | x | x | x | x |  Pins marked with a "x" should not be used.  
//            Usable Pins       +---+---+---+---+---+
//                              | 7 | x | 5 | 6 | 4 |  
//                              +---+---+---+---+---+

#define rightLineSensor	4		//PF4 - Bottom Right pin
#define leftLineSensor	5		//PF5 - 2 to the left of PF4	
#define RightDistSensor	6		//PF6 - 1 to the left of PF4 	
#define LeftDistSensor	7		//PF7 - Bottom Left pin 	

#define msLoop			5		// to control loop speed
#define msWaitStart		3000	// number of milliseconds to wait before start
#define sumOffLine		100		// off the line if sensor sum is below this
#define pwmDrive		9		// PWM speed for driving
#define DistThreshold	112		// Threshold for Distance sensor

volatile unsigned int accelRaw;
volatile unsigned int gyroRaw;

extern volatile char KEY_VALID;

// Variable from "button.c" to prevent button-bouncing
extern unsigned char gButtonTimeout;


int main (void) 
{ 
    Initialization();

    show_mhz();				// Display CPU speed
    TimerWait(1000);		// Wait 1 second

    while(1) 
	{
		printLCD("Show LR ADC");		// Display the text in quotes on the LCD
        while (!KEY_VALID);				// Wait for joystick to be moved or pressed.
        if (getkey() == 1)				// If enter was pressed then do what is in the braces, just skip over it. 
		{   
			TimerWait(500);				// debounce joystick button
            showADC();						                   
			printLCD("Back ADCs");
            TimerWait(2000);
		}
  
        printLCD("LED");				// Display the text in quotes on the LCD
        while (!KEY_VALID);				// Wait for joystick to be moved or pressed.
        if (getkey() == 1)				// If enter was pressed then do what is in the braces, else just continue.
		{	
			TimerWait(500);				// debounce joystick button
			led_sensor();                  
			printLCD("Back LED");
            TimerWait(2000);
		}
  
        printLCD("Servo");				// Display the text in quotes on the LCD
        while (!KEY_VALID);				// Wait for joystick to be moved or pressed.
        if (getkey() == 1)				// If enter was pressed then do what is in the braces, else just continue.
		{	
			TimerWait(500);				// debounce joystick button
            servo_test();                   
			printLCD("Back Servo");
            TimerWait(2000);
		}
  
		printLCD("Fast Loop");			// Display the text in quotes on the LCD
        while (!KEY_VALID);				// Wait for joystick to be moved or pressed.
        if (getkey() == 1)				// If enter was pressed then do what is in the braces, else just continue.
        {   
			TimerWait(500);				// debounce joystick button
            FastLoop();
			printLCD("Back Fast Loop");
            TimerWait(3000);
        }

        printLCD("Line Follow");		// Display the text in quotes on the LCD
        while (!KEY_VALID);				// Wait for joystick to be moved or pressed.
        if (getkey() == 1)				// If enter was pressed then do what is in the braces, else just continue.
		{	
			TimerWait(500);				// debounce joystick button
            LineFollow();
			printLCD("Back Line Follow");
            TimerWait(3000);
        }
  
        printLCD("PWM Test");			// Display the text in quotes on the LCD
        while (!KEY_VALID);				// Wait for joystick to be moved or pressed.
        if (getkey() == 1)				// If enter was pressed then do what is in the braces, else just continue.
        {
			TimerWait(500);				// debounce joystick button
            PWM_Test();
			printLCD("Back PWM Test");
            TimerWait(3000);
        }
  
        printLCD("USART Test");			// Display the text in quotes on the LCD
        while (!KEY_VALID);				// Wait for joystick to be moved or pressed.
        if (getkey() == 1)				// If enter was pressed then do what is in the braces, else just continue.
		{
            TimerWait(500);				// debounce joystick button
            usart_test();
			printLCD("Back USART Test");
            TimerWait(3000);
		}
			
        printLCD("PCF8574 Test");		// Display the text in quotes on the LCD
        while (!KEY_VALID);				// Wait for joystick to be moved or pressed.
        if (getkey() == 1)				// If enter was pressed then do what is in the braces, else just continue.
		{
            TimerWait(500);				// debounce joystick button
            PCF8574_test();
			printLCD("Back PCF8574 Test");
            TimerWait(3000);
        }

        printLCD("DS1803 Test");		// Display the text in quotes on the LCD
        while (!KEY_VALID);				// Wait for joystick to be moved or pressed.
        if (getkey() == 1)				// If enter was pressed then do what is in the braces, else just continue.
		{
            TimerWait(500);				// debounce joystick button
            DS1803_test();
			printLCD("Back DS1803");
            TimerWait(3000);
        }

        printLCD("GPIO14 Test");		// Display the text in quotes on the LCD
        while (!KEY_VALID);				// Wait for joystick to be moved or pressed.
        if (getkey() == 1)				// If enter was pressed then do what is in the braces, else just continue.
		{
            TimerWait(500);				// debounce joystick button
            GPIO14_test();
			printLCD("Back GPIO14 Test");
            TimerWait(3000);
        }

        printLCD("EEPROM Test");		// Display the text in quotes on the LCD
        while (!KEY_VALID);				// Wait for joystick to be moved or pressed.
        if (getkey() == 1)				// If enter was pressed then do what is in the braces, else just continue.
		{
            TimerWait(500);				// debounce joystick button
            EEPROM_test();
			printLCD("Back EEPROM Test");
            TimerWait(3000);
        }
    }   
}  


//*****************************************************************************
//*  showADC - Displays ADC reading on the LCD display. 
//*****************************************************************************  
void showADC(void) 
{
    unsigned char adcL, adcR = 0;
   
    SetupLineSensors();
    InitADC();
   
    while (!(getkey() == 1))
	{	
		//do the following block until enter is pressed
        adcL = GetADC(leftLineSensor);
        adcR = GetADC(rightLineSensor);
		LCD_ShowColons(1);
        show3hex(adcL, adcR, adcR);
        TimerWait(500);
    }
	LCD_ShowColons(0);
}


//*****************************************************************************
//*  Fast loop - should be started with one sensor on the black line and the
//*              other sensor off the line.
//*****************************************************************************  
void FastLoop(void) 
{
    unsigned char adcL, adcR;
	unsigned char Threshold;
   
    disable_JTAG();				// Making Port F available for line sensors and such.
    SetupLineSensors();			// Setup the line sensors
    InitADC();					// Initialize the ADCs for the sensors
   
    // read line sensor values
    adcL = GetADC(leftLineSensor);
    adcR = GetADC(rightLineSensor);

    Threshold = (adcL + adcR) / 2;
  
    while (!(getkey() == 1))
    {
        // read line sensor values
        adcL = GetADC(leftLineSensor);
        adcR = GetADC(rightLineSensor);

		if (adcL < Threshold)  
			SetLeftMotorPWM(10);
		else
			SetLeftMotorPWM(3);

		if (adcR < Threshold)
			SetRightMotorPWM(10);
		else
			SetRightMotorPWM(3);
    }
    // Done
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
}


//*****************************************************************************
//*		LineFollow - 
//*****************************************************************************  
void LineFollow(void)
{
    unsigned char adcL, adcR;
    int sensorDifference;
   
    SetupLineSensors();				// Setup the line sensors
    InitADC();						// Initialize the ADCs for the sensors
   
    while (!(getkey() == 1))		//do the folling block until enter is pressed
	{
		/* read line sensor values */
		adcL = GetADC(leftLineSensor);
		adcR = GetADC(rightLineSensor);

		/* calculate sum and difference */
		sensorDifference = (adcL - adcR) / 16;

		//if (GetADC(RightDistSensor) < DistThreshold)
		//{
			/* steer in proportion to the difference between the sensors */
			SetLeftMotorPWM(pwmDrive - sensorDifference);
			SetRightMotorPWM(pwmDrive + sensorDifference);
		//}
		//else
		//{
		//	SetLeftMotorPWM(0);
		//	SetRightMotorPWM(0);
		//}
	}
    // Done
	SetLeftMotorPWM(0);
    SetRightMotorPWM(0);
}


//*****************************************************************************
//* LED - Sensor
//*****************************************************************************  
void led_sensor(void) 
{
    InitADC();
   
	int temp1 = 0;
	int temp2 = 0;
   
  
	ClearBit(PORTF, PF6);	// turn bit "off" 
	SetBit(DDRF, DDF6);		// make it an output  

    while (!(getkey() == 1))
	{	
		temp1 = GetADC10(5);
		show12bits(temp1, temp2);

		if (temp1 > 0x100)
		{
			temp2++;

			ADCSRA &= (0<<ADEN);	// Disable ADC		
			ClearBit(PORTF, PF5);	// turn bit "off" 
			SetBit(DDRF, DDF5);		// make it an output
			SetBit(PORTF, PF6);		// turn bit "on" 

  			TimerWait(1000);

			ClearBit(PORTF, PF6);	// turn bit "off" 
			ClearBit(DDRF, DDF5);	// make it an input
			ADCSRA |= (1<<ADEN);	// enable ADC
		}
		TimerWait(12);
	}
}



//******************************************** 
//* PWM test - check wiring and such
//******************************************** 
void PWM_Test(void) 
{
    while (!(getkey() == 1)) 
	{
 	    printLCD("LFWD");
	    SetLeftMotorPWM(10);
        SetRightMotorPWM(0);
        TimerWait(3000);
 
        printLCD("LREV");
	    SetLeftMotorPWM(-10);
        SetRightMotorPWM(0);
        TimerWait(3000);
    
        printLCD("RFWD");
	    SetLeftMotorPWM(0);
        SetRightMotorPWM(10);
        TimerWait(3000);
	
        printLCD("RREV");
	    SetLeftMotorPWM(0);
        SetRightMotorPWM(-10);
        TimerWait(3000);
	
        printLCD("2 FWD low");
		SetRightMotorPWM(5);
	    SetLeftMotorPWM(5);
	    TimerWait(3000);

        printLCD("2 REV med");
	    SetRightMotorPWM(-8);
	    SetLeftMotorPWM(-8);
	    TimerWait(3000);
    
        printLCD("CCW");
		SetRightMotorPWM(8);
	    SetLeftMotorPWM(-8);
	    TimerWait(3000);

        printLCD("CW");
	    SetRightMotorPWM(-8);
	    SetLeftMotorPWM(8);
	    TimerWait(3000);
	}
    // Done
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
}

 
//******************************************** 
//* Test Servo 
//******************************************** 
void servo_test(void) 
{ 
	init_servo();			// Initialize Servo
	servo_center();			// center servo (send 1.5ms pulses)
	TimerWait(5000);		// Wait 5 seconds (5000 ms)

    while (!(getkey() == 1)) 
	{ 
         servo_up();		// turn 45deg CCW from center 
		 TimerWait(2000);	// Wait 2 seconds (2000 ms)

         servo_down();		// turn 45deg CW from center 
		 TimerWait(2000);	// Wait 2 seconds (2000 ms)
    }  
}



//******************************************** 
//* USART Test  
//******************************************** 
void usart_test(void) 
{ 
//	USART_Init(103);		// Initialize USART 9.6Kb
	USART_Init(51);			// Initialize USART 19.2Kb
//	USART_Init(25);			// Initialize USART 38.4Kb
//	USART_Init(3);			// Initialize USART 57.6Kb (2.1% error)

    while (!(getkey() == 1)) 
	{
		sendString("Hello World\r\n");
	//	TimerWait(100);		// Wait 1 seconds (100 ms)
    }  
}


//******************************************** 
//* PCF8574 Test  
//******************************************** 
void PCF8574_test(void) 
{ 
	I2C_Init();
    
	while (!(getkey() == 1)) 
	{
		PCF8574_write(5);
		TimerWait(20);		// Wait 100 ms
		PCF8574_write(10);
		TimerWait(20);		// Wait 100 ms
    }
}


//******************************************** 
//* DS1803 Test  
//******************************************** 
void DS1803_test(void) 
{ 
	I2C_Init();
    
	while (!(getkey() == 1)) 
	{
		DS1803_write(0,127);	// Set pot0 VCC/2)
		TimerWait(5000);		// Wait 1000 ms
		DS1803_write(1,255);	// Set to VCC
		TimerWait(5000);		// Wait 1000 ms
		DS1803_write(0,0);		// Set pot0 to 0
		TimerWait(5000);		// Wait 1000 ms
		DS1803_write(1,0);		// Set pot1 to 0
		TimerWait(5000);		// Wait 1000 ms
    }
}

//******************************************** 
//* GPIO14 Test  
//******************************************** 
#define CMD			0		// write only - command register
#define RESULT		1		// read only - result of A/D
#define AD_CONTROL	3		// read/write - ADCON1
#define GET_AD0		0x03	// Read A/D channels
#define GET_AD1		0x04
#define GET_AD2		0x05
#define GET_AD3		0x06
#define GET_AD4		0x07
#define PORT_A		4		// read/write
#define PORT_B		5		// read/write
#define SET_RB0		0x18
#define CLR_RB0		0x28
#define TOG_RB0		0x38

void GPIO14_test(void) 
{ 
	char temp1, temp2;
	int	result1 = 0; 
	int result2 = 0;

	I2C_Init();
    
//	temp1 = gpio_read1(CMD);		//	Read Firmware Version	
//	temp2 = 0;
//	show12bits(temp1, temp2);
//	TimerWait(1000);

	gpio_write(AD_CONTROL, 0X80);	//	Select right justification and all AN0-AN4 as analog inputs.
//	gpio_write(AD_CONTROL, 0X00);	//	Select left justification and all AN0-AN4 as analog inputs.

	while (!(getkey() == 1)) 
	{

/*	Things Tested and worked		*/
/*	the pin will automatically be set to output regardless of its previous state. */ 

		//	Setting bits works
//		gpio_write(CMD, SET_RB0);
//		TimerWait(100);
//		gpio_write(CMD, CLR_RB0);
//		TimerWait(100);

		//	Toggling Bits works
//		gpio_write(CMD, TOG_RB0);
//		TimerWait(100);

		//	Read port (8 bits) works
//		temp1 = gpio_read1(PORT_A); 
//		show12bits(temp1, result2++);
//		TimerWait(100);


		// Read ADC 8 bits with bits shifted left works 
		gpio_write(AD_CONTROL, 0X00);	//	Select left justification and all AN0-AN4 as analog inputs.
		gpio_write(CMD, GET_AD0);
		TimerWait(20);
		result1 = gpio_read1(RESULT);
		TimerWait(20);

		gpio_write(AD_CONTROL, 0X80);	//	Select right justification and all AN0-AN4 as analog inputs.
		gpio_write(CMD, GET_AD1);
		TimerWait(20);
		result2 = gpio_read2(RESULT);
		show12bits(result1, result2);
		TimerWait(20);
    }
}

//******************************************** 
//*	24LC256 Test  
//******************************************** 
void EEPROM_test(void) 
{ 
	I2C_Init();
    
	while (!(getkey() == 1)) 
	{
		PCF8574_write(5);
		TimerWait(20);		// Wait 100 ms
		PCF8574_write(10);
		TimerWait(20);		// Wait 100 ms
    }
}
